#include "PointCloudFilterTable.h"

/*!
 * Destructor function.
 * @param sigma. The distance in mm that is allowed for points to support the 
 * plane. Once the dominant plane is found, all point above sigma distance
 * from the plane are include in the collision-info file.
 * @param r1. The minimum distance from the base of the robot. Points closer are 
 * not considered.
 * @param r2. The maximum distance from the base of the robot. Points further are
 * not considered.
 * @param horizontalAssumtion. Boolean indicating whether a (more-or-less) horizontal
 * plane is assumed or whether planes of all orientations can be found. 
 */
PointCloudFilterTable::PointCloudFilterTable(double _sigma, double _r1, double _r2, bool _horizontalAssumption) {
  minNrRANSACIterations = 50;
  maxNrRANSACIterations = 150;
  minSamples = 3;
  sigma = _sigma; // 15, Points further from the table are excluded
  r1 = _r1*_r1;   // All Points closer are not considered. Use square for faster calculation
  r2 = _r2*_r2;   // All Points further are not considered. Use square for faster calculation
  horizontalAssumption = _horizontalAssumption;
  planeEquation.estimated = false;
}

PointCloudFilterTable::~PointCloudFilterTable() {
}

void PointCloudFilterTable::setParameters(double _sigma, bool _horizontalAssumption) {
  sigma = _sigma; // 15, Texlets further from the table are excluded
  horizontalAssumption = _horizontalAssumption;
}

void PointCloudFilterTable::fitTablePlane(PointCloud *pointCloud) {
  //vector<Texlet*> inputSet;
  //for(size_t i=0; i<texlets.size(); i++) 
  //  inputSet.push_back( texlets[i] );

  TablePlane tablePlaneFitting(minNrRANSACIterations, maxNrRANSACIterations, sigma, minSamples, horizontalAssumption);
  tablePlaneFitting.setInputSet(pointCloud->points);
  tablePlaneFitting.fitModel();


  vector<double> tablePlaneVector = tablePlaneFitting.getTablePlaneVector();
  planeEquation.a = tablePlaneVector[0];
  planeEquation.b = tablePlaneVector[1];
  planeEquation.c = tablePlaneVector[2];
  planeEquation.d = tablePlaneVector[3];
  planeEquation.nABC = sqrt(planeEquation.a*planeEquation.a + planeEquation.b*planeEquation.b + planeEquation.c*planeEquation.c);
  planeEquation.estimated = true;

  //cout << "Plane: " << planeEquation.a << ", " << planeEquation.b << ", " << 
  //  planeEquation.c << ", " << planeEquation.d << endl;

}

PointCloud* PointCloudFilterTable::filter(PointCloud* pointCloudIn) {
  if(!planeEquation.estimated)
    fitTablePlane(pointCloudIn);

  PointCloud* inliers = new PointCloud(PC_SHALLOW_COPY);

  if(planeEquation.c >= 0) { // Normal points downwards, away from the camera, D>0 is above the table
    for(size_t i=0; i<pointCloudIn->points.size(); i++) {
      if(includeUpward(pointCloudIn->points[i]->pos[0], pointCloudIn->points[i]->pos[1], pointCloudIn->points[i]->pos[2]))
	inliers->points.push_back( pointCloudIn->points[i] );
    }
  }
  else { // c<0, Normal points upwards, towards the camera, D<0 is above the table
    for(size_t i=0; i<pointCloudIn->points.size(); i++) {
      if(includeDownward(pointCloudIn->points[i]->pos[0], pointCloudIn->points[i]->pos[1], pointCloudIn->points[i]->pos[2]))
	inliers->points.push_back( pointCloudIn->points[i] );
    }
  }
  return inliers; 
}


/*!
 * Should this point be included. This implements the case where the normal of the table plane 
 * points towards the camera
 */
inline bool PointCloudFilterTable::includeUpward(double x, double y, double z) {
  double D,R;
  double a = planeEquation.a;
  double b = planeEquation.b;
  double c = planeEquation.c;
  double d = planeEquation.d;
  double nABC = planeEquation.nABC;
  R = x*x + y*y + z*z;
  if(R>=r1 && R<=r2) {
    D = (x*a + y*b + z*c + d) / nABC;
    if(D>sigma)
      return true;
    else
      return false;
  }
  else
    return false;
}

/*!
 * Should this point be included. This implements the case where the normal of the table plane 
 * points away from the camera
 */
inline bool PointCloudFilterTable::includeDownward(double x, double y, double z) {
  double D,R;
  double a = planeEquation.a;
  double b = planeEquation.b;
  double c = planeEquation.c;
  double d = planeEquation.d;
  double nABC = planeEquation.nABC;
  R = x*x + y*y + z*z;
  if(R>=r1 && R<=r2) {
    D = (x*a + y*b + z*c + d) / nABC;
    if(D<-sigma)
      return true;
    else
      return false;
  }
  else
    return false;
}
